// Include headers from OS
#include <joyos.h>
#include <Math.h>
#include "navigate5.h"
#include "gather.h"
#include "deposit.h"
#include "strategy.h"


extern volatile uint8_t robot_id;


// usetup is called during the calibration period. 
int usetup (void) {
	gyro_init(20, 1400000, 1000); // gyro is on the 20th analogue pin
	robot_id = 21;
    return 0;
}

// Entry point to contestant code.
int umain (void) {

	uint32_t start_time = get_time();
	uint8_t mySection = inSection(getMyPosition());
	
	if (mySection == 1){ //1 is Blue 0 is Red (hack)
		setMyColor(0);	
	}else if (mySection == 4){
		setMyColor(1);
	}
	
	
	exploreTerritories();
	
	while(get_time() - start_time < 90000){
		gatherSheep(chooseNextTerritory());
	}
	
	depositSheep();
	
	while(1){
		gatherSheep(chooseNextTerritory);
	}
	
	/*while(1){
		printf("myX: %f \n", getMyPosition().x);
		printf("myY: %f \n", getMyPosition().y);
		printf("myHeading: %f \n", getMyHeading());
		pause(5000);
	}*/
	return 0;
}
